import roslib; roslib.load_manifest('object_manager')
import rosbag
import sys
import rgb_to_name
import matplotlib.pyplot as plt
import numpy as np

def extract_hue(bag):
    print "Extracting object lists:"
    ol_count = 0;
    hs = []
    #for topic, msg, t in bag.read_messages(topics=['/blocknlp/object_list']):
    for topic, msg, t in bag.read_messages(topics=['/mit/object_list']):
        try:
            c = msg.object_list[0].color
            (h,s,v) = rgb_to_name.rgb_to_hsv(c.r,c.g,c.b)
            hs.append(h)
        except:
            print "x"
        ol_count = ol_count + 1;
    print "   - extracted %d object lists\n"%(ol_count)
    return hs

plot_colors = {
    'blue'  : 'b',
    'green' : 'g',
    'red'   : 'r',
    'cyan'  : 'c',
    'magenta' : 'm',
    'yellow' : 'y',
    'black' : 'k',
    'white' : 'w'
}

def main():
    hue_data = {}
    print "Got %d arguments"%(len(sys.argv)-1)
    for i in range(1, len(sys.argv), 2):
        color_char = sys.argv[i]
        bag_fname = sys.argv[i+1]
        if color_char not in plot_colors.values():
            print "ERROR: matplotlib color char %s not valid!!!!!"%(color_char)
            print "Valid colors:"
            print plot_colors.values()
            exit()
        print "processing color (%s), bagfile: %s\n"%(color_char, bag_fname)
        bag = rosbag.Bag(bag_fname)
        hs = extract_hue(bag)
        bag.close()
        n = np.array(hs)
        hue_data[color_char] = n

    plt.hold(True)
    for k in hue_data.keys():
        plt.hist( hue_data[k], color=k, normed=True ) #numbins=100?
    plt.savefig('color_hist.png')
    plt.show()



    #keyboard()

if __name__=="__main__":
    main()
